#ifndef MOTOR_CONTROL_HPP
#define MOTOR_CONTROL_HPP
#include <string>
#include <vector>
#include <iostream>
#include "serial.hpp"
#include "config.hpp"
#define LIMIT_0 25
class MotorControl {
public:
    MotorControl(std::string port, int baud)
         {
        serial_.open(port, baud); 
        // 设置串口回调，将收到的数据转发给本类的recvMsg
        serial_.set_receive_callback([this](const std::vector<uint8_t>& msg) {
            this->recvMsg(const_cast<std::vector<uint8_t>&>(msg));
        });
        startTimer(); // 启动定时器
      //  running_ = false; // 初始化运行状态
      //  serial_.open();
    }
    ~MotorControl() {
        stopTimer();
        serial_.close();
    }
    void setMotorControl(Motor_Config &config) { config_ = config; }
    int rec_ok(std::vector<u_int8_t> &msg);
    void recvMsg(std::vector<u_int8_t> &msg);
    void get_recv_msg(std::vector<u_int8_t> &msg);
    int get_send(int task_switch);
    void startTimer(void) {
        if (running_) return;
        running_ = true;
        //int ms = 10; // Default interval
        
        timer_thread_ = std::thread([this]() {
            while (running_) {
                //std::string hello = "hello";
                sendbuf_vect.clear(); // Clear the send buffer before sending
                delay_ms = get_send(task_switch); // Example delay, can be set dynamically
                //int delay_ms = 10; // Default delay in milliseconds
                if(sendbuf_vect.size() > 0) { // Check if there is data to send
                    //std::cout << "Sending data: " << sendbuf_vect.size() << " bytes" << std::endl;
                    serial_.send(sendbuf_vect);
                } 
                
                std::chrono::milliseconds interval;
               // delay_ms = 20; // Example delay, can be set dynamically
                interval = std::chrono::milliseconds(delay_ms);
                std::this_thread::sleep_for(interval);
                task_switch++;
                if(task_switch>12){
                    task_switch=0;
                  //  config_.yaokonglun();
                }
              //  std::cout << "Timer tick, delay: " << delay_ms << " ms" << std::endl;
            }
        });
    }
    void stopTimer() {
        running_ = false;
        if (timer_thread_.joinable()) timer_thread_.join();
    }
    SerialPort serial_;
    int get_motor_rec_times(int i) {
        return config_.motor_info[i].rec_times;
    }
    int get_yaokong_rec_times() {
        return config_.yaokong_times;
    }
    int get_bianma_rec_times(int i) {
        return config_.bianma_rectimes[i];
    }
    Motor_Config config_; // Serial configuration
    void set_module_speed(void);
private:
    int set_motor_send(int nob);
    int set_bianma_send(int nob);
    int set_yaokong_send(int nob);
    int cal_angle_run(int agg,int zero);
    
    std::vector<u_int8_t> sendbuf_vect; // Buffer for sending data
    std::vector<u_int8_t> recvbuf_vect; // Buffer for receiving data
    int motor_speed[8] = {0}; // Array to hold motor speeds
    std::thread timer_thread_;
    bool running_=false; // Flag to indicate if the timer is running
    int delay_ms = 10; // Default delay in milliseconds
    int task_switch = 0; // Task switch variable
    std::vector<u_int8_t> rec_allmsg;
};
int MotorControl::rec_ok(std::vector<u_int8_t> &msg)
{
    int flag=0;
     u_int8_t ch=0;
    if(msg[0]==0xaa)  //can 口接收数据
    {
      if(msg.size()<12)flag= 0;
      for(int i=0;i<11;i++)
        ch+=msg[i];
        if(ch==msg[11])flag=1;  
    }
    if(msg[0]==0xbb)    //can 口接收数据
    {
      if(msg.size()<12)flag= 0;
      for(int i=0;i<11;i++)
        ch+=msg[i];
        if(ch==msg[11])flag=2;  
    }  
    if(msg[0]==0xcc)  //串口口接收数据
    {
      if(msg.size()<12)flag= 0;
      for(int i=0;i<10;i++)
        ch+=msg[i];
       if(ch==msg[10])
        flag=3;  
    }
  
        return flag;
   
}
void MotorControl::get_recv_msg(std::vector<u_int8_t> &msg)
{
    int16_t id,ag;
    if(rec_ok(msg) != 0) {
         id=msg[1]*256+msg[2];
        for(int i=0;i<8;i++)    //读取电机速度
        {
            if((config_.motor_drv[i].Id+0x100)==id)
            {
                ag=msg[3]*256+msg[4];
                config_.motor_info[i].speed_cur=ag*config_.motor_drv[i].direction;
                config_.motor_info[i].rec_times++;
              //  std::cout<< "Received motor data: ID=" << id << ", Speed=" << ag << std::endl;
            }
        }
    
        for(int i=0;i<4;i++)    //读取编码器值
        {
           if((config_.bianma_drv[i].Id+0x100)==id)
            {
                ag=msg[3]*256+msg[4];
                config_.bianma_cur[i]=cal_angle_run(ag,config_.bianma_zero[i]);
                config_.bianma_rectimes[i]++;
               // std::cout<< "Received bianma data: ID=" << id << ", Angle=" << ag << std::endl;
            }
        }
        if(msg[0]==0xcc)  //串口口接收数据
        {
            int value1;
            if((int)msg[9]==config_.yaokong_channel)
            {
                config_.yaokong_times++;
                value1=(int)msg[2]-128;
                if(value1>=0)
                    {
                        if(value1<LIMIT_0)value1=0;
                        else value1=value1-LIMIT_0;
                    }
                    else 
                    {
                        if(value1>-LIMIT_0)value1=0;
                        else value1=value1+LIMIT_0;
                    }
                config_.yaokong_value.Left_horiz=-value1;
    //////////////////////////  
                    value1=(int)msg[3]-128;
                    if(value1>=0)
                        {
                            if(value1<LIMIT_0)value1=0;
                            else value1=value1-LIMIT_0;
                        }
                        else 
                        {
                            if(value1>-LIMIT_0)value1=0;
                            else value1=value1+LIMIT_0;
                        }
                    config_.yaokong_value.Left_vertic=-value1;
      ///////////////////
                    value1=(int)msg[4]-128;
                    if(value1>=0)
                        {
                            if(value1<LIMIT_0)value1=0;
                            else value1=value1-LIMIT_0;
                        }
                        else 
                        {
                            if(value1>-LIMIT_0)value1=0;
                            else value1=value1+LIMIT_0;
                        }
                    config_.yaokong_value.Right_horiz=-value1;
       ///////////////////
                    value1=(int)msg[5]-128;
                    if(value1>=0)
                        {
                            if(value1<LIMIT_0)value1=0;
                            else value1=value1-LIMIT_0;
                        }
                        else 
                        {
                            if(value1>-LIMIT_0)value1=0;
                            else value1=value1+LIMIT_0;
                        }
                    config_.yaokong_value.Right_vertic=-value1;
                    config_.yaokong_value.key1=msg[6]; config_.yaokong_value.key2=msg[7];
    }
           
            }
         }
    }
void MotorControl::recvMsg(std::vector<u_int8_t> &msg)
{
    rec_allmsg.insert(rec_allmsg.end(), msg.begin(), msg.end()); // Append received message to the buffer
    while (rec_allmsg.size() >= 12) { // Process messages in chunks of 12 bytes
        {
            if (rec_ok(rec_allmsg) != 0) {
                get_recv_msg(rec_allmsg);
                rec_allmsg.erase(rec_allmsg.begin(), rec_allmsg.begin() + 12);
            }
            else rec_allmsg.erase(rec_allmsg.begin()); // Remove processed message
        }
     }
}

int MotorControl::get_send(int task_switch)
{
     int16_t id,delay;
        u_int8_t ch=0;
      switch (task_switch) {
        case 0:
            delay=set_motor_send(0);
            break;
        case 1:
            delay=set_motor_send(1);
            break;
        case 2:
            delay=  set_motor_send(2);
            break;
        case 3:
            delay=  set_motor_send(3);
            break;
        case 4:
            delay=  set_motor_send(4);
            break;
        case 5:
            delay=  set_motor_send(5);
            break;
        case 6:
            delay=  set_motor_send(6);
            break;
        case 7:
            delay=  set_motor_send(7);
            break;
        case 8:
            delay=  set_bianma_send(0);
            break;
        case 9:
            delay=  set_bianma_send(1);
            break;
        case 10:
            delay=  set_bianma_send(2);
            break;    
        case 11:
            delay=  set_bianma_send(3);
            break;
        case 12:
            delay=  set_yaokong_send(0);
            break;
        default:
            return 2; // Default delay
    }
    return delay;
}
int MotorControl::set_motor_send(int nob){
    int id,delay;
    u_int8_t ch=0;
            sendbuf_vect.push_back(0xaa);
            id=config_.motor_drv[nob].Id;
            delay=config_.motor_drv[nob].delay;
            sendbuf_vect.push_back((id>>8)&0xff);
            sendbuf_vect.push_back(id&0xff);
            sendbuf_vect.push_back(0x00);
        int16_t speed_set = config_.motor_info[nob].speed_set*config_.motor_drv[nob].direction;    
            sendbuf_vect.push_back(speed_set>>8); // 高字节
            sendbuf_vect.push_back(speed_set&0xff); // 低字节
            sendbuf_vect.push_back(0x00);
            

            sendbuf_vect.push_back(0x00);
            sendbuf_vect.push_back(0x00);
            sendbuf_vect.push_back(0x00);
            sendbuf_vect.push_back(0x00);
            for(int i=0;i<11;i++)
              ch+=sendbuf_vect[i];
            sendbuf_vect.push_back(ch);
            return delay;
}
int MotorControl::set_bianma_send(int nob){
    int id,delay;
    u_int8_t ch=0;
            sendbuf_vect.push_back(0xaa);
            id=config_.bianma_drv[nob].Id;
            delay=config_.bianma_drv[nob].delay;
            sendbuf_vect.push_back((id>>8)&0xff);
            sendbuf_vect.push_back(id&0xff);
            sendbuf_vect.push_back(0x00);      
            sendbuf_vect.push_back(0x00);
            sendbuf_vect.push_back(0x00);
            sendbuf_vect.push_back(0x00);

            sendbuf_vect.push_back(0x00);
            sendbuf_vect.push_back(0x00);
            sendbuf_vect.push_back(0x00);
            sendbuf_vect.push_back(0x00);
            for(int i=0;i<11;i++)
              ch+=sendbuf_vect[i];
            sendbuf_vect.push_back(ch);
            return delay;
}
int MotorControl::set_yaokong_send(int nob){
    int id,delay;
    u_int8_t ch=0;
    delay=2;
            sendbuf_vect.push_back(0xcc);
            sendbuf_vect.push_back(0x0a);
            sendbuf_vect.push_back(0x00);
            sendbuf_vect.push_back(0x00);
            sendbuf_vect.push_back(0x00);
            sendbuf_vect.push_back(0x00);

            sendbuf_vect.push_back(0x00);
            sendbuf_vect.push_back(0x00);
            sendbuf_vect.push_back(0x00);
            sendbuf_vect.push_back(0x00);
            for(int i=0;i<10;i++)
              ch+=sendbuf_vect[i];
            sendbuf_vect.push_back(ch);
           sendbuf_vect.push_back(0xbb);
            return delay;
}
int MotorControl::cal_angle_run(int agg,int zero_ag)  //计算角度
{//左手定律
  //int zero_ag=-15374-1060;
  int angle,temp;
  if(zero_ag>=0)
  {
    if(agg>=0)
    {
        angle=agg-zero_ag;
    }
    else 
    {
        temp=zero_ag-32768;
        if(temp<=agg){ angle=agg-zero_ag;}
        else
        {
            angle= 32768+agg-temp;
        }
    }
  }
  else
  {
    if(agg>=0)  //zero_ag<0 agg>0
    {
      temp=32768+zero_ag; 
      if(temp>=agg)
       {angle=agg-zero_ag;}
      else
      {
        angle=agg-32768-temp;
      } 
     
    }
    else  //zero_ag<0 agg<0
    {
      angle=agg-zero_ag;  
    }
  }
  return angle;
}
#define LIMIT_static_angle 500
#define LIMIT_static_speed 900
void MotorControl::set_module_speed(void)
{
        int tiao,temp;
        //motor_module
     for(int i=0;i<4;i++)
     {
        config_.motor_module[i].angle_cur=config_.bianma_cur[i];
        temp=config_.motor_module[i].angle_set-config_.motor_module[i].angle_cur;
        tiao=temp/24;
        if(tiao>200)tiao=200;
        if(tiao<-200)tiao=-200;
        if(config_.motor_module[i].module_speed_set==0) //如果模块速度为0
        {
            if((temp>0&&temp<LIMIT_static_angle)||(temp<0&&temp>-LIMIT_static_angle))
            {
               tiao=0;
            }
                //continue;
        }    
            config_.motor_module[i].speed_set_L=config_.motor_module[i].module_speed_set+tiao;
            config_.motor_module[i].speed_set_R=config_.motor_module[i].module_speed_set-tiao;
        if(config_.motor_module[i].speed_set_L>LIMIT_static_speed)config_.motor_module[i].speed_set_L=LIMIT_static_speed;
        if(config_.motor_module[i].speed_set_L<-LIMIT_static_speed)config_.motor_module[i].speed_set_L=-LIMIT_static_speed;
        if(config_.motor_module[i].speed_set_R>LIMIT_static_speed)config_.motor_module[i].speed_set_R=LIMIT_static_speed;
        if(config_.motor_module[i].speed_set_R<-LIMIT_static_speed)config_.motor_module[i].speed_set_R=-LIMIT_static_speed;
        config_.motor_info[i*2].speed_set=config_.motor_module[i].speed_set_L;
        config_.motor_info[i*2+1].speed_set=config_.motor_module[i].speed_set_R;
     }

}
#endif
  
